
#include "MapObstacle.h"
#include "Utils/Util.h"
#include "Settings.h"

#include <GL/gl.h>
#include <iostream>
#include <cassert>
#include <cmath>


MapObstacle::MapObstacle(Vector2D world_min_coord, Vector2D world_max_coord, 
                         float obstacle_expand_size_tight, float obstacle_expand_size_loose) :
    world_min_coord(world_min_coord), 
    world_max_coord(world_max_coord),
    obstacle_expand_size_tight(obstacle_expand_size_tight),
    obstacle_expand_size_loose(obstacle_expand_size_loose) {

}

MapObstacle::~MapObstacle(){

}

bool MapObstacle::load(const std::vector<int> &obstacle_map, int width, int height, int label){
    for(int y = 0; y < height; y++){
        for(int x = 0; x < width; x++){
            assert(x >= 0 && x < width);
            assert(y >= 0 && y < height);

            if(obstacle_map[x + y*width] == label && 
               !isEdgePixel(obstacle_map, width, height, label, x, y)){
                inside_points.push_back(pixelToPosition(x, y, width, height));
            }
        }
    }

    half_grid_length = 0.5f*(world_max_coord.x - world_min_coord.y)/width;

    int original_x, original_y;
    int pixel_x, pixel_y, prev_pixel_x, prev_pixel_y;

    if(!findEdgePixel(obstacle_map, width, height, label, pixel_x, pixel_y)){ return false; }
    corners.push_back(pixelToPosition(pixel_x, pixel_y, width, height));

    prev_pixel_x = original_x = pixel_x;
    prev_pixel_y = original_y = pixel_y;

    if(!findNeighbouringEdgePixel(obstacle_map, width, height, label, 
                                  prev_pixel_x, prev_pixel_y, prev_pixel_x, prev_pixel_y, pixel_x, pixel_y)){
        return false;
    }

    corners.push_back(pixelToPosition(pixel_x, pixel_y, width, height));


    int new_pixel_x = -1, new_pixel_y = -1;
    while(!(new_pixel_x == original_x && new_pixel_y == original_y)){
        if(!findNeighbouringEdgePixel(obstacle_map, width, height, label, 
                                      pixel_x, pixel_y, prev_pixel_x, prev_pixel_y, new_pixel_x, new_pixel_y)){
            return false;
        }

        prev_pixel_x = pixel_x;
        prev_pixel_y = pixel_y;

        pixel_x = new_pixel_x;
        pixel_y = new_pixel_y;

        corners.push_back(pixelToPosition(pixel_x, pixel_y, width, height));
    }

    // Find the bounding box of the obstacle
    for(unsigned i = 0; i < corners.size(); i++){
        if(i == 0 || corners[i].x < obstacle_min_coord.x){ obstacle_min_coord.x = corners[i].x; }
        if(i == 0 || corners[i].y < obstacle_min_coord.y){ obstacle_min_coord.y = corners[i].y; }
        if(i == 0 || corners[i].x > obstacle_max_coord.x){ obstacle_max_coord.x = corners[i].x; }
        if(i == 0 || corners[i].y > obstacle_max_coord.y){ obstacle_max_coord.y = corners[i].y; }
    }

    simplifyPolygon();
    generateNormals();
    generateCollisionObstacle(obstacle_expand_size_tight, collision_space_corners_tight);
    generateCollisionObstacle(obstacle_expand_size_loose, collision_space_corners_loose);

    if(Settings::instance().getIntValue("graphics", "render")){
        generateDisplayList();
    }
    return true;
}


void MapObstacle::render(void){
    glCallList(dl_id);
}


bool MapObstacle::pointInside(Vector2D point, bool use_collision_space, 
                              bool tight_collision_space){

    // create a line segment starting at the point and going out to "infinity" to one side.
    // then check to see how many times this line segment intersects the walls of the polygon.
    // if it intersects an odd number of times, then the point is inside the polygon, if an 
    // even number of times, then the point is outside the polygon.
   
    std::vector<Vector2D> check_corners;
    if(use_collision_space){
        if(tight_collision_space){
            check_corners = collision_space_corners_tight;
        }   
        else{
            check_corners = collision_space_corners_loose;
        }
    }
    else{
        check_corners = corners;
    }
 
    unsigned num_intersects = 0;
    Vector2D start = point, end = point + Vector2D(2.0f*(obstacle_max_coord.x-obstacle_min_coord.x), 0.0f);
    Vector2D ipoint;

    for(unsigned i = 0; i < check_corners.size(); i++){
        Vector2D wall_start = check_corners[i];
        Vector2D wall_end   = check_corners[(i + 1)%check_corners.size()];

        if(Util::linesIntersect(start, end, wall_start, wall_end, ipoint)){
            num_intersects++;
        }
    }

    return (num_intersects%2) == 1; // point is inside if intersects is odd
}


bool MapObstacle::collideRaySegment(Vector2D start, Vector2D end, Vector2D &inormal, 
                                    Vector2D &ipoint, bool use_collision_space, bool tight_collision_space){
    float closest_dist = 0.0f;
    Vector2D closest_normal, closest_ipoint;
    bool collision = false;


    std::vector<Vector2D> check_corners;
    if(use_collision_space){
        if(tight_collision_space){
            check_corners = collision_space_corners_tight;
        }   
        else{
            check_corners = collision_space_corners_loose;
        }
    }
    else{
        check_corners = corners;
    }


    for(unsigned i = 0; i < check_corners.size(); i++){
        Vector2D wall_start = check_corners[i];
        Vector2D wall_end   = check_corners[(i + 1)%check_corners.size()];
        
        Vector2D cp;
        if(Util::linesIntersect(start, end, wall_start, wall_end, cp)){
            float dist = (cp - start).length();
            if(!collision || dist < closest_dist){
                closest_dist = dist;
                closest_normal = normals[i];
                closest_ipoint = cp;
            }
            
            collision = true;
        }
    }

    if(collision){
        inormal = closest_normal;
        ipoint = closest_ipoint;
        return true;
    }

    return false;
}


bool MapObstacle::collideSphere(Vector2D centre, float radius, Vector2D &normal, Vector2D &collision_point,
                                Vector2D &constraint_free_point){

    bool result = false;
    unsigned num_collisions = 0;
    constraint_free_point = Vector2D(0.0f, 0.0f);

    for(unsigned i = 0; i < corners.size(); i++){
        Vector2D wall_start = corners[i];
        Vector2D wall_end   = corners[(i + 1)%corners.size()];
        Vector2D cp;        

        float dist = fabs(Util::pointSegmentDist(centre, wall_start, wall_end, cp));
        if(dist < radius){
            normal = normals[i];
            collision_point = cp;

            // Constrain the position of the ship to be outside the obstacle
            constraint_free_point = centre + (radius*1.01f - dist)*normals[i];
            centre = constraint_free_point;         

            num_collisions++;
            result = true;
        }
    }
    
    if(num_collisions > 0){
        Vector2D ncollision_point, nconstraint_free_point;
        if(collideSphere(centre, radius, normal, ncollision_point, nconstraint_free_point)){
            collision_point = ncollision_point;
            constraint_free_point = nconstraint_free_point;
        }
    }

    return result;
}


void MapObstacle::generateDisplayList(void){
    dl_id = glGenLists(1);
    glNewList(dl_id,GL_COMPILE);
    
    // Render the obstacle into the display list.
    glPushMatrix();
   
    // Render the inside of the obstacle as a grid of points.
    glColor3f(0.2f, 0.2f, 0.2f);
    glPointSize(1.0f);
    glBegin(GL_POINTS);    
    for(unsigned i = 0; i < inside_points.size(); i++){
        glVertex3f(inside_points[i].x, inside_points[i].y, 0.0f);
    }
    glEnd();
 

    // Render the outline of the obstacle
    glColor3f(1.0f, 1.0f, 1.0f);
    glBegin(GL_LINE_STRIP);
    for(unsigned i = 0; i < corners.size(); i++){
        glVertex3f(corners[i].x, corners[i].y, 0.0f);
    }
    glVertex3f(corners[0].x, corners[0].y, 0.0f);
    glEnd();

    // Render the collision obstacle
    /*glColor3f(0.6f, 0.2f, 0.2f);
    glBegin(GL_LINE_STRIP);
    for(unsigned i = 0; i < collision_space_corners_tight.size(); i++){
        glVertex3f(collision_space_corners_tight[i].x, collision_space_corners_tight[i].y, 0.0f);
    }
    glVertex3f(collision_space_corners_tight[0].x, collision_space_corners_tight[0].y, 0.0f);
    glEnd();


    glColor3f(0.2f, 0.2f, 0.6f);
    glBegin(GL_LINE_STRIP);
    for(unsigned i = 0; i < collision_space_corners_loose.size(); i++){
        glVertex3f(collision_space_corners_loose[i].x, collision_space_corners_loose[i].y, 0.0f);
    }
    glVertex3f(collision_space_corners_loose[0].x, collision_space_corners_loose[0].y, 0.0f);
    glEnd();
    */

    glPopMatrix();
    glEndList();
}


void MapObstacle::generateNormals(void){
    for(unsigned i = 0; i < corners.size(); i++){
        Vector2D wall_start = corners[i];
        Vector2D wall_end   = corners[(i + 1)%corners.size()];
        
        Vector2D wall_ray = wall_end - wall_start;
        wall_ray.normalise();

        // We take the wall normal pointing one way, if it points out of the 
        // polygon, thats the normal we want. If it points into the polygon, 
        // we flip the normal and use that.

        Vector2D wall_normal = wall_ray;
        wall_normal.rotate(90.0f);

        int inside_votes = 0;

        for(unsigned j = 0; j < 3; j++){
            Vector2D test_point = wall_start + Util::rand_interval(0.0f, 1.0f)*(wall_end - wall_start);
            test_point = test_point + (half_grid_length * wall_normal);
            if(pointInside(test_point, false, false)){
                inside_votes++;
            }
            else{
                inside_votes--;
            }
        }
        if(inside_votes > 0){
            wall_normal.scale(-1.0f);
        }

        normals.push_back(wall_normal);
    }
}


void MapObstacle::generateCollisionObstacle(float size_of_object, std::vector<Vector2D> &result_buffer){
    for(unsigned i = 0; i < corners.size(); i++){
        Vector2D cur_normal = normals[i];
        Vector2D next_normal = normals[(i+corners.size()-1)%corners.size()];
        Vector2D projected_normal = cur_normal + next_normal;
        projected_normal.normalise();

        float diff = acosf(cur_normal.dotProduct(next_normal));
        float overestimate = 1.2f;
        float l = (size_of_object*overestimate)/cos(diff/2.0f);

        projected_normal.scale(l);
        
        Vector2D new_vertex = corners[i] + projected_normal;
        result_buffer.push_back(new_vertex);
    }
}


void MapObstacle::simplifyPolygon(void){
    std::vector<Vector2D> simple_polygon;
    Vector2D cur  = corners[0];
    Vector2D next = corners[1];    

    for(unsigned i = 2; i < corners.size(); i++){
        Vector2D vec1 = next - cur;
        vec1.normalise();

        Vector2D vec2 = corners[i] - cur;
        vec2.normalise();
        
        if(fabs(vec1.dotProduct(vec2) - 1.0f) > 0.00001f){
            simple_polygon.push_back(cur);
            cur  = corners[i-1];
            next = corners[i];
        }
    }
    simple_polygon.push_back(cur);
    corners = simple_polygon;
}

bool MapObstacle::findNeighbouringEdgePixel(const std::vector<int> &obstacle_map, int width, int height, int label, 
                                            int cx, int cy, int notx, int noty, int &x, int &y){

    if(isEdgePixel(obstacle_map, width, height, label, cx+1, cy+1) && !(notx == cx+1 && noty == cy+1)){
        if(haveCommonEmptyNeighbours(obstacle_map, width, height, cx, cy, cx+1, cy+1)){
            x = cx + 1; y = cy + 1; return true;
        }
    }
    if(isEdgePixel(obstacle_map, width, height, label, cx+1, cy-1) && !(notx == cx+1 && noty == cy-1)){
        if(haveCommonEmptyNeighbours(obstacle_map, width, height, cx, cy, cx+1, cy-1)){
            x = cx + 1; y = cy - 1; return true;
        }
    }
    if(isEdgePixel(obstacle_map, width, height, label, cx+1, cy) && !(notx == cx+1 && noty == cy)){
        if(haveCommonEmptyNeighbours(obstacle_map, width, height, cx, cy, cx+1, cy)){
            x = cx + 1; y = cy; return true;
        }
    }


    if(isEdgePixel(obstacle_map, width, height, label, cx-1, cy+1) && !(notx == cx-1 && noty == cy+1)){
        if(haveCommonEmptyNeighbours(obstacle_map, width, height, cx, cy, cx-1, cy+1)){
            x = cx - 1; y = cy + 1; return true;
        }
    }
    if(isEdgePixel(obstacle_map, width, height, label, cx-1, cy-1) && !(notx == cx-1 && noty == cy-1)){
        if(haveCommonEmptyNeighbours(obstacle_map, width, height, cx, cy, cx-1, cy-1)){
            x = cx - 1; y = cy - 1; return true;
        }
    }
    if(isEdgePixel(obstacle_map, width, height, label, cx-1, cy) && !(notx == cx-1 && noty == cy)){
        if(haveCommonEmptyNeighbours(obstacle_map, width, height, cx, cy, cx-1, cy)){
            x = cx - 1; y = cy; return true;
        }
    }


    if(isEdgePixel(obstacle_map, width, height, label, cx, cy+1) && !(notx == cx && noty == cy+1)){
        if(haveCommonEmptyNeighbours(obstacle_map, width, height, cx, cy, cx, cy+1)){
            x = cx; y = cy + 1; return true;
        }
    }
    if(isEdgePixel(obstacle_map, width, height, label, cx, cy-1) && !(notx == cx && noty == cy-1)){
        if(haveCommonEmptyNeighbours(obstacle_map, width, height, cx, cy, cx, cy-1)){
            x = cx; y = cy - 1; return true;
        }
    }

    return false;
}


bool MapObstacle::findEdgePixel(const std::vector<int> &obstacle_map, int width, int height, int label, int &x, int &y){
    for(y = 0; y < height; y++){
        for(x = 0; x < width; x++){
            if(isEdgePixel(obstacle_map, width, height, label, x, y)){ return true; }
        }
    }

    return false;
}


bool MapObstacle::isEdgePixel(const std::vector<int> &obstacle_map, int width, int height, int label, int x, int y){
    // we use a 4 neighbour approach for deciding if a pixel should be considered part of the perimeter. 
    // this gives us a smoother perimeter.
    if(x < 0 || x >= width){ return false; }
    if(y < 0 || y >= height){ return false; }

    assert(x >= 0 && x < width);
    assert(y >= 0 && y < height);
    if(obstacle_map[x + y*width] != label){ return false; }

    return isEmptyPixel(obstacle_map, width, height, x-1, y) || 
           isEmptyPixel(obstacle_map, width, height, x+1, y) ||
           isEmptyPixel(obstacle_map, width, height, x, y-1) || 
           isEmptyPixel(obstacle_map, width, height, x, y+1);
}


bool MapObstacle::isEmptyPixel(const std::vector<int> &obstacle_map, int width, int height, int x, int y){
    if(x < 0 || x >= width || y < 0 || y >= height){ return true; }
    else if(obstacle_map[x + y*width] == 0){ return true; }
    else{ return false; }
}


bool MapObstacle::haveCommonEmptyNeighbours(const std::vector<int> &obstacle_map, int width, int height, 
                                            int x1, int y1, int x2, int y2){

    if(isEmptyPixel(obstacle_map, width, height, x1-1, y1+1) && isNeighbourOf(x1-1, y1+1, x2, y2)){ return true; }
    if(isEmptyPixel(obstacle_map, width, height, x1-1, y1-1) && isNeighbourOf(x1-1, y1-1, x2, y2)){ return true; }
    if(isEmptyPixel(obstacle_map, width, height, x1-1, y1) && isNeighbourOf(x1-1, y1, x2, y2)){ return true; }

    if(isEmptyPixel(obstacle_map, width, height, x1+1, y1+1) && isNeighbourOf(x1+1, y1+1, x2, y2)){ return true; }
    if(isEmptyPixel(obstacle_map, width, height, x1+1, y1-1) && isNeighbourOf(x1+1, y1-1, x2, y2)){ return true; }
    if(isEmptyPixel(obstacle_map, width, height, x1+1, y1) && isNeighbourOf(x1+1, y1, x2, y2)){ return true; }

    if(isEmptyPixel(obstacle_map, width, height, x1, y1+1) && isNeighbourOf(x1, y1+1, x2, y2)){ return true; }
    if(isEmptyPixel(obstacle_map, width, height, x1, y1-1) && isNeighbourOf(x1, y1-1, x2, y2)){ return true; }
    
    return false;
}


bool MapObstacle::isNeighbourOf(int x, int y, int tx, int ty){
    return (int)abs(x-tx) <= 1 && (int)abs(y-ty) <= 1;
}

Vector2D MapObstacle::pixelToPosition(int pixel_x, int pixel_y, int width, int height){
    float xfrac = (float)pixel_x/(float)(width-1);
    float yfrac = (float)pixel_y/(float)(height-1);

    float xwidth = world_max_coord.x - world_min_coord.x;
    float ywidth = world_max_coord.y - world_min_coord.y;

    return Vector2D(xfrac*xwidth + world_min_coord.x, yfrac*ywidth + world_min_coord.y);
}

